#include "ano_tc.h"

/**
 * @brief  发送8位数据到上位机
 * @param  Fun    功能值
 * @param  Data   发送的数据 
 * @retval 无
 */
void ano_tx_uint8(uint8_t Fun, uint8_t data)
{
  uint8_t tx_buf[6];
	char num;											//循环变量
  tx_buf[5] = 0;    						//校验位复位
  tx_buf[0] = 0xAA;         	 	//帧头
  tx_buf[1] = 0xAA;          		//帧头
  tx_buf[2] = Fun;           		//功能
  tx_buf[3] = 0x01;        			//长度
	tx_buf[4] = data;        			//数据
  tx_buf[5] = 0xAA + 0xAA + Fun + 0x01 + data;			//计算校验位
  for(num=0; num<6; num++)                          //发送数据
		HAL_UART_Transmit(&ano_uart, &tx_buf[num], 1, 0xff);
}

/**
 * @brief  发送16位数据到上位机
 * @param  Fun    功能值
 * @param  Data   发送的数据 
 * @retval 无
 */
void ano_tx_uint16(uint8_t Fun, uint16_t data)
{
	unsigned char tx_buf[7];
	char num;											//循环变量
  tx_buf[5] = 0;    						//校验位复位
  tx_buf[0] = 0xAA;          		//帧头
  tx_buf[1] = 0xAA;          		//帧头
  tx_buf[2] = Fun;           		//功能
  tx_buf[3] = 0x02;        			//长度
	tx_buf[4] = (data&0xff00)>>8;	//数据高位
	tx_buf[5] = data&0x00FF;  		//数据低位
  tx_buf[6] = 0xAA + 0xAA + Fun + 0x02 + tx_buf[4] + tx_buf[5];
  for(num=0; num<7; num++) {                         //发送数据
		HAL_UART_Transmit(&ano_uart, &tx_buf[num], 1, 0xff);
	}
}

/**
 * @brief  发送任意位数据到上位机
 * @param  Fun    功能值
 * @param  Data   发送的数据 
 * @param	 tx_len 发送长度
 * @retval 无
 */
void ano_tx(uint8_t Fun, uint16_t* data, uint8_t tx_len)
{
  uint8_t tx_buf[32];
	uint8_t i;
  tx_buf[tx_len + 4] = 0;    //校验位复位
  tx_buf[0] = 0xAA;          //帧头
  tx_buf[1] = 0xAA;          //帧头
  tx_buf[2] = Fun;           //功能
  tx_buf[3] = tx_len;        //长度
	
	/* 计算数据，高位在前低位在后 */
  for(i=0; i<tx_len; i++) {
    if((i%2) == 0) {
      tx_buf[i +4] = (data[i/2]&0xff00)>>8;
    } else {
      tx_buf[i +4] = data[i/2]&0x00ff; 
		}
  }
	
	/* 计算校验位 */
  for(i=tx_len+4; i>0; i--) {                 
    tx_buf[tx_len + 4] += tx_buf[(tx_len + 4) - i];
	}
	
  /* 发送数据 */
  for(char num=0;num<(tx_len+5);num++) {                           
		HAL_UART_Transmit(&ano_uart, &tx_buf[num], 1, 0xff);
	}
  
}

/*****************************
  * @ROL				俯仰角
  * @PIT				横滚角
  * @YAW				偏航角
  * @HIGH			高度
  * @FLY_MODE	飞行模式
  * @ARMED			是否解锁
  *返回值: 无
  ****************************
  */
void ano_tx_state(float roll, float pitch, float yaw, int32_t hight, uint8_t fly_mod, uint8_t fly_lock)
{
	uint16_t tx_buf[6];
	tx_buf[0] = (signed short)roll*100;
	tx_buf[1] = (signed short)pitch*100;
	tx_buf[2] = (signed short)yaw*100;
	
	tx_buf[3] = (signed short)(hight&0xffff0000)>>16;
	tx_buf[4] = (signed short)hight&0x0000ffff;
	tx_buf[5] = (unsigned short)((fly_mod&&1)<<8)|(fly_lock&&1);
	ano_tx(0X01, tx_buf, 12);
}




